/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.archwood.frc2607;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.DigitalInput;

/**
 *
 *Author: John
 */
public class SensorThreadOfDoom extends Counter implements Runnable{
    Counter ofDoom;
    Thread me;
    double whatiexpecttoberpm = 0;
    double sixvalues = 0;
    double intervaltime = 0;
    double samplingArray[] = {0,0,0,0,0};
    public SensorThreadOfDoom(int i)
    {
    super (new DigitalInput(i));
    //me = new Thread(this);
    setUpSourceEdge(false,true);
    setMaxPeriod(2);
   // me.start();
    }
    public void setMaxTime(double d)
    {

        intervaltime = d;
    }
    public double getRPM()
    {
        return (1000/((samplingArray[4]+samplingArray[3]+samplingArray[1]+samplingArray[2]+samplingArray[0])/5))*60;
    }
    public void run() {
        sixvalues=System.currentTimeMillis();
        while (true)
        {
            double r = System.currentTimeMillis();
            if (r-sixvalues>intervaltime)//r and sixvalues are in ms, intervaltime es en secundos
            {
            reset();
            samplingArray[0]=0;
            samplingArray[1]=0;
            samplingArray[2]=0;
            samplingArray[3]=0;
            samplingArray[4]=0;
            sixvalues=r;
            }
            if (get()>=1)
            {
                samplingArray[0]=samplingArray[1];
                samplingArray[1]=samplingArray[2];
                samplingArray[2]=samplingArray[3];
                samplingArray[3]=samplingArray[4];
                samplingArray[4] = r-sixvalues;

                sixvalues = r;
                reset();
            }
            try {
                Thread.sleep(1);
            }
            catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
    }

}
